package com.gitee.wsl.mathematics.geometry.d2.contour.rectify

import com.gitee.wsl.mathematics.geometry.d2.contour.ShapeContour
import com.gitee.wsl.mathematics.geometry.d2.contour.ext.fromContours
import com.gitee.wsl.mathematics.geometry.d2.contour.segment.Segment2D
import com.gitee.wsl.mathematics.geometry.d2.contour.segment.ext.mix
import com.gitee.wsl.mathematics.matrix.mat4.Mat4f
import com.gitee.wsl.mathematics.vector.vec2.Vec2f
import kotlin.math.floor

class RectifiedContour(contour: ShapeContour, distanceTolerance: Double = 0.5, lengthScale: Double = 1.0) :
    RectifiedPath<Vec2f>(contour, distanceTolerance, lengthScale) {
    fun velocity(t: Double): Vec2f {
        return if (originalPath.empty) {
            Vec2f.ZERO
        } else {
            val (segment, st) = originalPath.segment(rectify(safe(t)).toFloat())
            originalPath.segments[segment].direction(st)
        }
    }

    fun normal(t: Double): Vec2f {
        return if (originalPath.empty) {
            Vec2f.UNIT_Y
        } else {
            (originalPath as ShapeContour).normal(rectify(safe(t)))
        }
    }

    fun pose(t: Double): Mat4f {
        originalPath as ShapeContour
        return if (originalPath.empty) {
            Mat4f.IDENTITY
        } else {
            originalPath.pose(rectify(safe(t)))
        }
    }

    override fun sub(t0: Double, t1: Double): ShapeContour {
        originalPath as ShapeContour
        if (originalPath.empty) {
            return ShapeContour.EMPTY
        }

        return if (originalPath.closed) {
            originalPath.sub(rectify(t0.mod(1.0)) + floor(t0), rectify(t1.mod(1.0)) + floor(t1))
        } else {
            originalPath.sub(rectify(t0), rectify(t1))
        }
    }

    override fun splitAt(ascendingTs: List<Double>, weldEpsilon: Double): List<ShapeContour> {
        @Suppress("UNCHECKED_CAST")
        return super.splitAt(ascendingTs, weldEpsilon) as List<ShapeContour>
    }

    val contour: ShapeContour get() = originalPath as ShapeContour
}

/** create a rectified contour
 * @param distanceTolerance distance tolerance to use, 0.5 is the default distance tolerance
 * @param lengthScale used to compute the size of the LUT, default value is 1.0
 **/
fun ShapeContour.rectified(distanceTolerance: Double = 0.5, lengthScale: Double = 1.0): RectifiedContour {
    return RectifiedContour(this, distanceTolerance, lengthScale)
}

/**  create a rectified contour
 *   @param distanceTolerance distance tolerance to use, 0.5 is the default distance tolerance
 *   @param lengthScale used to compute the size of the LUT, default value is 1.0
 *
 * */
fun Segment2D.rectified(distanceTolerance: Double = 0.5, lengthScale: Double = 1.0): RectifiedContour {
    return RectifiedContour(this.contour, distanceTolerance, lengthScale)
}


/**
 * Split for blending with [other]
 */
fun RectifiedContour.splitForBlend(other: RectifiedContour): RectifiedContour {
    val ts = (0 until other.originalPath.segments.size + 1).map { it.toDouble() / other.originalPath.segments.size }
    val rts = ts.map { other.inverseRectify(it) }

    return ShapeContour.fromContours(splitAt(rts), originalPath.closed && other.originalPath.closed).rectified()
}

fun RectifiedContour.mix(other: RectifiedContour, blendFunction: (Double) -> Double): ShapeContour {
    val n = this.originalPath.segments.size.toDouble()
    val segs = (this.originalPath.segments zip other.originalPath.segments).mapIndexed { index, it ->
        val t0 = inverseRectify(index / n)
        val t1 = inverseRectify((index + 1 / 3.0) / n)
        val t2 = inverseRectify((index + 2 / 3.0) / n)
        val t3 = inverseRectify((index + 1) / n)
        (it.first as Segment2D).mix(it.second as Segment2D, blendFunction(t0), blendFunction(t1), blendFunction(t2), blendFunction(t3))
    }
    return ShapeContour.fromSegments(segs, originalPath.closed && other.originalPath.closed)
}